#ifndef _REBO_REPLAN_FSM_H_
#define _REBO_REPLAN_FSM_H_

#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <vector>
#include <visualization_msgs/Marker.h>

#include <bspline_opt/bspline_optimizer.h>
#include <plan_env/grid_map.h>
#include <traj_utils/Bspline.h>
#include <traj_utils/MultiBsplines.h>
#include <geometry_msgs/PoseStamped.h>
#include <plan_manage/planner_manager.h>
#include <traj_utils/planning_visualization.h>

using std::vector;

namespace ego_planner {

class EGOReplanFSM
{

private:
  /* ---------- flag ---------- */
  enum FSM_EXEC_STATE {
    INIT,
    WAIT_TARGET,
    GEN_NEW_TRAJ,
    REPLAN_TRAJ,
    EXEC_TRAJ,
    EMERGENCY_STOP,
    SEQUENTIAL_START
  };
  enum TARGET_TYPE {
    MANUAL_TARGET = 1,
    PRESET_TARGET = 2,
    REFENCE_PATH = 3
  };

  string state_str[8] = {"INIT",
                         "WAIT_TARGET",
                         "GEN_NEW_TRAJ",
                         "REPLAN_TRAJ",
                         "EXEC_TRAJ",
                         "EMERGENCY_STOP",
                         "SEQUENTIAL_START"};

  /* planning utils */
  PlanManager::Ptr planner_manager_;
  Visualization::Ptr visualization_;
  traj_utils::MultiBsplines multi_bspline_msgs_buf_;

  /* parameters */
  int target_type_; // 1 mannual select, 2 hard code
  double thresh_no_replan_meter_, thresh_replan_time_;
  double waypoints_[50][3];
  int waypoint_num_;
  double planning_horizen_, planning_horizen_time_;
  double emergency_time_;
  bool flag_realworld_experiment_;
  bool enable_fail_safe_;

  /* planning data */
  bool have_trigger_, have_target_, have_odom_, have_new_target_, have_recv_pre_agent_;
  bool receive_target_traj_;
  FSM_EXEC_STATE exec_state_;
  int continously_called_times_{0};

  Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_; // odometry state
  Eigen::Quaterniond odom_orient_;

  Eigen::Vector3d start_pt_, start_vel_, start_acc_, start_yaw_;                      // start state
  Eigen::Vector3d end_pt_, end_vel_;                                                  // goal state
  Eigen::Vector3d local_target_pt_, local_target_vel_, predict_target_, predict_vel_; // local target state
  int current_wp_;

  bool flag_NOT_emergency_;
  bool yaw_plan_success_;
  int stop_time_;

  /* ROS utils */
  ros::NodeHandle node_;
  ros::Timer exec_timer_, safety_timer_, yaw_timer_;
  ros::Subscriber waypoint_sub_, odom_sub_, swarm_trajs_sub_, broadcast_bspline_sub_, trigger_sub_;
  ros::Publisher replan_pub_, new_pub_, bspline_pub_, swarm_trajs_pub_, broadcast_bspline_pub_;

  ros::Publisher pos_list_pub_, cpt_list_pub_, yaw_list_pub_, attract_list_pub_, attract_score_list_pub_, debug_cpt_list_pub_, gradient_list_pub_, pk_grad_list_pub_;
  ros::Publisher new_predict_list_pub_;
  ros::Publisher yaw_pub_;
  /* helper functions */
  bool callReplan(bool flag_use_poly_init, bool flag_randomPolyTraj); // front-end and back-end method
  bool callEmergencyStop(Eigen::Vector3d stop_pos);                   // front-end and back-end method
  bool planFromGlobalTraj(const int trial_times = 1);
  bool planFromCurrentTraj(const int trial_times = 1);

  /* return value: std::pair< Times of the same state be continuously called, current continuously called state > */
  void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call);
  std::pair<int, EGOReplanFSM::FSM_EXEC_STATE> timesOfConsecutiveStateCalls();
  void printFSMExecState();

  void planGlobalTrajbyGivenWps();
  void getLocalTarget();
  int findExtremumSumIndex(const std::vector<double> &data, int length, const bool isMin);

  /* ROS functions */
  void execFSMCallback(const ros::TimerEvent &e);
  void checkCollisionCallback(const ros::TimerEvent &e);
  void planYawCallback(const ros::TimerEvent &e);
  void waypointCallback(const nav_msgs::PathConstPtr &msg);
  void waypointCallback_forWP(const nav_msgs::PathConstPtr &msg);
  void triggerCallback(const geometry_msgs::PoseStampedPtr &msg);
  void odometryCallback(const nav_msgs::OdometryConstPtr &msg);
  void swarmTrajsCallback(const traj_utils::MultiBsplinesPtr &msg);
  void BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg);

  void broadcastTrajs(bool startup_pub);

public:
  EGOReplanFSM(/* args */)
  {
  }
  ~EGOReplanFSM()
  {
  }

  void init(ros::NodeHandle &nh);

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace ego_planner

#endif